//
// Created by craipy on 23-2-7.
//

#ifndef KALMAN_LINEARSYSTEMMODEL_HPP
#define KALMAN_LINEARSYSTEMMODEL_HPP

#include "kalman/SystemModel.hpp"
#include "kalman/StandardBase.hpp"

namespace Kalman {
    template<class StateType>
    class LinearKalmanFilter;

    template<class StateType, class ControlType = Vector<typename StateType::Scalar, 0>>
    class LinearSystemModel : public SystemModel<StateType, ControlType, StandardBase> {
        friend class LinearKalmanFilter<StateType>;

    protected:
        Matrix<typename StateType::Scalar, StateType::RowsAtCompileTime, StateType::RowsAtCompileTime> A;
        Matrix<typename StateType::Scalar, StateType::RowsAtCompileTime, ControlType::RowsAtCompileTime> B;
        using StandardBase<StateType>::P;

    public:
        //! System model base
        typedef SystemModel<StateType, ControlType, StandardBase> Base;

        //! System state type
        using typename Base::State;

        //! System control input type
        using typename Base::Control;

        State f(const State &x, const Control &u) const override {
            return A * x + B * u;
        }
    };

}// Kalman

#endif //KALMAN_LINEARSYSTEMMODEL_HPP
